/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */

class GenericPhysicalMotor;

#include "PhysicalJoint.h"
#include "PhysicalMotor.h"
#include "FixedJoint.h"
#include "BallJoint.h"
#include "HingeJoint.h"
#include "UniversalJoint.h"
#include "Physics.h"

using std::vector;
using std::map;

float GenericPhysicalJoint::factor = M_PI/180.0;

template<>
void PhysicalJoint<FixedJoint>::update(dJointID jointID) {
}

template<>
void PhysicalJoint<BallJoint>::update(dJointID jointID) {
	BallJoint *thiz = dynamic_cast<BallJoint*>(this);
	
	vector<Motor*>& motors = thiz->getMotors();
	for(unsigned i = 0; i < motors.size(); i++) {
		thiz->getMotors()[i]->setAngle(dJointGetAMotorAngle(jointID, i));
		dynamic_cast<GenericPhysicalMotor*>(thiz->getMotors()[i])->animateJoint(this, jointID, i);
	}
}

template<>
void PhysicalJoint<HingeJoint>::update(dJointID jointID) {
	HingeJoint *thiz = dynamic_cast<HingeJoint*>(this);

	vector<Motor*>& motors = thiz->getMotors();
	for(unsigned i = 0; i < motors.size(); i++) {
		thiz->getMotors()[i]->setAngle(dJointGetHingeAngle(jointID));
		dynamic_cast<GenericPhysicalMotor*>(thiz->getMotors()[i])->animateJoint(this, jointID, i);
	}
}

template<>
void PhysicalJoint<UniversalJoint>::update(dJointID jointID) {
	UniversalJoint *thiz = dynamic_cast<UniversalJoint*>(this);

	vector<Motor*>& motors = thiz->getMotors();
	for(unsigned i = 0; i < motors.size(); i++) {
		thiz->getMotors()[i]->setAngle(i==0?dJointGetUniversalAngle1(jointID):dJointGetUniversalAngle2(jointID));
		dynamic_cast<GenericPhysicalMotor*>(thiz->getMotors()[i])->animateJoint(this, jointID, i);
	}
}

template<>
void PhysicalJoint<FixedJoint>::setVelocityAndForce(dJointID jointID, int axisNumber, double velocity, double force) {
}

template<>
void PhysicalJoint<BallJoint>::setVelocityAndForce(dJointID jointID, int axisNumber, double velocity, double force) {
	dJointSetAMotorParam(jointID, dParamVel + axisNumber * dParamGroup, velocity);
	dJointSetAMotorParam(jointID, dParamFMax + axisNumber * dParamGroup, force);
}

template<>
void PhysicalJoint<HingeJoint>::setVelocityAndForce(dJointID jointID, int axisNumber, double velocity, double force) {
	dJointSetHingeParam(jointID, dParamVel + axisNumber* dParamGroup, velocity);
	dJointSetHingeParam(jointID, dParamFMax + axisNumber* dParamGroup, force);
}

template<>
void PhysicalJoint<UniversalJoint>::setVelocityAndForce(dJointID jointID, int axisNumber, double velocity, double force) {
	dJointSetUniversalParam(jointID, dParamVel + axisNumber * dParamGroup, velocity);
	dJointSetUniversalParam(jointID, dParamFMax + axisNumber * dParamGroup, force);
}

/* Note about the methods below - in ODE torques added by dJointAdd*MotorTorques are just
 * wrappers to dBodyAddTorque. Torques added by dBodyAddTorque function are accumulative,
 * so setting torque for all axes by calling dJointAdd*MotorTorques for single axes should
 * be perfectly fine. */

template<>
void PhysicalJoint<FixedJoint>::setTorque(dJointID jointID, int axisNumber, double torque) {
}

template<>
void PhysicalJoint<BallJoint>::setTorque(dJointID jointID, int axisNumber, double torque) {
	switch(axisNumber) {
		case 0:
		dJointAddAMotorTorques(jointID, torque, 0, 0);
		break;
		case 1:
		dJointAddAMotorTorques(jointID, 0, torque, 0);
		break;
		case 2:
		dJointAddAMotorTorques(jointID, 0, 0, torque);
		break;
	}
}

template<>
void PhysicalJoint<HingeJoint>::setTorque(dJointID jointID, int axisNumber, double torque) {
	dJointAddHingeTorque(jointID, torque);
}

template<>
void PhysicalJoint<UniversalJoint>::setTorque(dJointID jointID, int axisNumber, double torque) {
	switch(axisNumber) {
		case 0:
		dJointAddUniversalTorques(jointID, torque, 0);
		break;
		case 1:
		dJointAddUniversalTorques(jointID, 0, torque);
		break;
	}
}

template<>
dJointID PhysicalJoint<FixedJoint>::init(dWorldID dynWorld, dBodyID body1, dBodyID body2, dVector3 globalPos, dMatrix3 globalRot) {
	FixedJoint *thiz = dynamic_cast<FixedJoint*>(this);
	dJointID jointID = dJointCreateFixed(dynWorld, 0);
	dJointAttach(jointID, body1, body2);
	dJointSetFixed(jointID);
	return jointID;
}

template<>
dJointID PhysicalJoint<BallJoint>::init(dWorldID dynWorld, dBodyID body1, dBodyID body2, dVector3 globalPos, dMatrix3 globalRot) {
	BallJoint *thiz = dynamic_cast<BallJoint*>(this);
	dJointID jointID = dJointCreateBall(dynWorld, 0);

    dVector3 localPos;
    Physics::convertVector(thiz->getAnchor(), localPos);
    dVector3 localPosRotated;
    dMULTIPLY0_331 (localPosRotated,globalRot,localPos);
    dVector3 newGlobalPos;
    dOP(newGlobalPos,+,globalPos,localPosRotated);
	
	dJointAttach(jointID, body1, body2);
	dJointSetBallAnchor(jointID, newGlobalPos[0], newGlobalPos[1], newGlobalPos[2]);

	jointID = dJointCreateAMotor(dynWorld, 0);
	dJointAttach(jointID, body1, body2);
	dJointSetAMotorNumAxes(jointID, 3);

	vector<Motor*>& motors = thiz->getMotors();
	for(unsigned i = 0; i < motors.size(); i++) {
		dVector3 axis;
		Physics::convertVector(thiz->getMotors()[i]->getAxis(), axis);
		dVector3 axisRotated;
	    dMULTIPLY0_331 (axisRotated,globalRot,axis);

		dJointSetAMotorAxis(jointID, i, (i==2?(body2==0||body1==0?0:2):1), axisRotated[0], axisRotated[1], axisRotated[2]);
		dJointSetAMotorParam(jointID, dParamLoStop + i * dParamGroup, thiz->getMotors()[i]->getLoStop()*M_PI/180);
		dJointSetAMotorParam(jointID, dParamHiStop + i * dParamGroup, thiz->getMotors()[i]->getHiStop()*M_PI/180);
		dJointSetAMotorParam(jointID, dParamFudgeFactor + i * dParamGroup, 0.1);
		dJointSetAMotorParam(jointID, dParamBounce + i * dParamGroup, thiz->getMotors()[i]->getBounciness());		
		dynamic_cast<GenericPhysicalMotor*>(thiz->getMotors()[i])->animateJoint(this, jointID, i);
	}

	dJointSetAMotorMode(jointID,  dAMotorEuler);
	dJointID retJoint = jointID;

	jointID = dJointCreateAMotor(dynWorld, 0);
	dJointAttach(jointID, body1, body2);
	dJointSetAMotorNumAxes(jointID, 3);

	dMatrix3 rot;
	dVector3 axis;
	Physics::convertVector(thiz->getMotors()[2]->getAxis(), axis);
	dVector3 axisRotated;
    dMULTIPLY0_331 (axisRotated,globalRot,axis);
	    
	dRFromAxisAndAngle (rot, axisRotated[0], axisRotated[1], axisRotated[2], M_PI/4);

	for(unsigned i = 0; i < motors.size(); i++) {
		dVector3 axis;
		Physics::convertVector(thiz->getMotors()[i]->getAxis(), axis);
		dVector3 axisRotated;
	    dMULTIPLY0_331 (axisRotated,globalRot,axis);
    
		if(i<2) {
			dVector3 axisRot;
			dMULTIPLY0_331 (axisRot,rot,axisRotated);

			dJointSetAMotorAxis(jointID, i, 1, axisRot[0], axisRot[1], axisRot[2]);
			dJointSetAMotorParam(jointID, dParamLoStop + i * dParamGroup, thiz->getMotors()[i]->getLoStop()*M_PI/180);
			dJointSetAMotorParam(jointID, dParamHiStop + i * dParamGroup, thiz->getMotors()[i]->getHiStop()*M_PI/180);
			dJointSetAMotorParam(jointID, dParamFudgeFactor + i * dParamGroup, 0.1);
			dJointSetAMotorParam(jointID, dParamBounce + i * dParamGroup, thiz->getMotors()[i]->getBounciness());		
		} else {
			dJointSetAMotorAxis(jointID, i, 1, axisRotated[0], axisRotated[1], axisRotated[2]);
		}
	}

	dJointSetAMotorMode(jointID,  dAMotorEuler);
	return retJoint;
}

template<>
dJointID PhysicalJoint<HingeJoint>::init(dWorldID dynWorld, dBodyID body1, dBodyID body2, dVector3 globalPos, dMatrix3 globalRot) {
	HingeJoint *thiz = dynamic_cast<HingeJoint*>(this);
	dJointID jointID = dJointCreateHinge(dynWorld, 0);

    dVector3 localPos;
    Physics::convertVector(thiz->getAnchor(), localPos);
    dVector3 localPosRotated;
    dMULTIPLY0_331 (localPosRotated,globalRot,localPos);
    dVector3 newGlobalPos;
    dOP(newGlobalPos,+,globalPos,localPosRotated);

	dJointAttach(jointID, body1, body2);
	dJointSetHingeAnchor(jointID, newGlobalPos[0], newGlobalPos[1], newGlobalPos[2]);
	
	dVector3 axis;
	Physics::convertVector(thiz->getMotors()[0]->getAxis(), axis);
	dVector3 axisRotated;
    dMULTIPLY0_331 (axisRotated,globalRot,axis);
    
	dJointSetHingeAxis(jointID, axisRotated[0], axisRotated[1], axisRotated[2]);
	dJointSetHingeParam(jointID, dParamLoStop, thiz->getMotors()[0]->getLoStop()*M_PI/180);
	dJointSetHingeParam(jointID, dParamHiStop, thiz->getMotors()[0]->getHiStop()*M_PI/180);
	dJointSetHingeParam(jointID, dParamFudgeFactor, 0.1);
	dJointSetHingeParam(jointID, dParamBounce, thiz->getMotors()[0]->getBounciness());		
	dynamic_cast<GenericPhysicalMotor*>(thiz->getMotors()[0])->animateJoint(this, jointID, 0);
	
	return jointID;
}

template<>
dJointID PhysicalJoint<UniversalJoint>::init(dWorldID dynWorld, dBodyID body1, dBodyID body2, dVector3 globalPos, dMatrix3 globalRot) {
	UniversalJoint *thiz = dynamic_cast<UniversalJoint*>(this);
	dJointID jointID = dJointCreateUniversal(dynWorld, 0);

    dVector3 localPos;
    Physics::convertVector(thiz->getAnchor(), localPos);
    dVector3 localPosRotated;
    dMULTIPLY0_331 (localPosRotated,globalRot,localPos);
    dVector3 newGlobalPos;
    dOP(newGlobalPos,+,globalPos,localPosRotated);

	dJointAttach(jointID, body1, body2);
	dJointSetUniversalAnchor(jointID, newGlobalPos[0], newGlobalPos[1], newGlobalPos[2]);
	
	vector<Motor*>& motors = thiz->getMotors();
	for(unsigned i = 0; i < motors.size(); i++) {
		dVector3 axis;
		Physics::convertVector(thiz->getMotors()[i]->getAxis(), axis);
		dVector3 axisRotated;
	    dMULTIPLY0_331 (axisRotated,globalRot,axis);
	
		if(i==0) dJointSetUniversalAxis1(jointID, axisRotated[0], axisRotated[1], axisRotated[2]);
		if(i==1) dJointSetUniversalAxis2(jointID, axisRotated[0], axisRotated[1], axisRotated[2]);
		dJointSetUniversalParam(jointID, dParamLoStop + i * dParamGroup, thiz->getMotors()[i]->getLoStop()*M_PI/180);
		dJointSetUniversalParam(jointID, dParamHiStop + i * dParamGroup, thiz->getMotors()[i]->getHiStop()*M_PI/180);
		dJointSetUniversalParam(jointID, dParamFudgeFactor + i * dParamGroup, 0.1);
		dJointSetUniversalParam(jointID, dParamBounce + i * dParamGroup, thiz->getMotors()[i]->getBounciness());		
		dynamic_cast<GenericPhysicalMotor*>(thiz->getMotors()[i])->animateJoint(this, jointID, i);
	}

	return jointID;
}
